/*
joint.h
joint navigation
designed by chenwenbo

-----------------------
2021-02-21 16:00:00
create file
-----------------------
*/

#ifndef __JOINT_H
#define __JOINT_H

#include "../../bsp/bsp.h"

#define JOINT_NUM	(3)					//关节数量

/*********************************************************/
#define M_PI		(3.14159265358f)
#define DEG2RAD		(M_PI / 180.0f)		//角度转弧度系数
#define RAD2DEG		(180 / M_PI)		//弧度转角度系数

#define IK_ALLOW_ERROR			(0.2f)	//逆解允许误差
#define IK_MAX_ITERATE_NUMS		(50)	//逆解最大迭代次数

/*********************************************************/
#define DEFAULT_JOINT_LENGTH	(2000)	//默认的关节长度
#define ALLOW_UPDATE_TIME_ERR	(3000)	//位置更新允许的最大时间差

/*********************************************************/
//洗车机管线架旋转轴与轮中心的偏移(以车头方向为X轴正方向)
#ifdef DRYER
//#define ROBOT_CENTER_OFFSET_Y	(129.0f)	//AP2020
//#define ROBOT_CENTER_OFFSET_X	(349.5f)

#define DEFAULT_HOME_POSE_X		(3200)	//默认待机位置X
#define DEFAULT_HOME_POSE_Y		(-1750)	//默认待机位置Y
#define DEFAULT_HOME_POSE_Z		(0)		//默认待机位置Z

#define ROBOT_CENTER_OFFSET_Y	(118.0f)	//AP2021A
#define ROBOT_CENTER_OFFSET_X	(348.5f)

#else
#define ROBOT_CENTER_OFFSET_Y	(-204.0f)
#define ROBOT_CENTER_OFFSET_X	(349.5f)
#define DEFAULT_HOME_POSE_X		(-1000)	//默认待机位置X
#define DEFAULT_HOME_POSE_Y		(-1750)	//默认待机位置Y
#define DEFAULT_HOME_POSE_Z		(0)		//默认待机位置Z
#endif
#define ROBOT_CENTER_ANGLE		(atan2(ROBOT_CENTER_OFFSET_Y, ROBOT_CENTER_OFFSET_X) * RAD2DEG)


/**********************************************************/
#define DEF_AREA_MAX_X			(8100)			//场地X方向最大距离默认值
#define DEF_AREA_MAX_Y			(5000)    		//场地Y方向最大距离默认值
#define DEF_AREA_MIN_X			(5000)         	//场地X方向最小距离默认值
#define DEF_AREA_MIN_Y			(2500)    		//场地Y方向最小距离默认值

#define DEF_RADIUS_LIMIT_MAX	(4500)			//默认半径限制最大值
#define DEF_RADIUS_LIMIT_MIN	(1200)			//默认半径限制最小值
#define DEF_ENC3_LIMIT_MAX		(320)			//默认编码器3限制最大值
#define DEF_ENC3_LIMIT_MIN		(100)			//默认编码器3限制最小值

#define MAP_GRID_WIDTH			(100)			//地图栅格宽度

//#define MAP_ROWS				(DEF_AREA_MAX_X / MAP_GRID_WIDTH)	//地图栅格行数
//#define MAP_COLUMNS			(DEF_AREA_MAX_Y / MAP_GRID_WIDTH)	//地图栅格列数

#define AREA_OFFSET_X			0               //场地X方向与管线架中心的偏移
#define AREA_OFFSET_Y			0               //场地Y方向与管线架中心的偏移

#define ROBOT_LIDAR_OFFSET_X	(640)     		//雷达相对机器人中心的Y偏移
#define ROBOT_LIDAR_OFFSET_Y	(-280)     		//雷达相对机器人中心的X偏移
#define ROBOT_LIDAR_OFFSET_Z	(90)     		//雷达坐标系相对机器人坐标系的角度偏移

#define ROBOT_WIDTH				(600)         	//机器人宽度
#define ROBOT_LENGTH			(1000)        	//机器人长度

/*************************************/
#define IS_X_POSITIVE			(0x01)
#define IS_X_NEGATIVE 			(0x02)
#define IS_Y_POSITIVE			(0x04)
#define IS_Y_NEGATIVE 			(0x08)
	
#define BIT_LF	            	(1 << 0)
#define BIT_LB	            	(1 << 1)
#define BIT_RB	            	(1 << 2)
#define BIT_RF	            	(1 << 3)
#define BIT_MIN_RADIUS	    	(1 << 4)
#define BIT_MAX_RADIUS	    	(1 << 5)
#define BIT_ENC3_MIN	    	(1 << 6)
#define BIT_ENC3_MAX	    	(1 << 7)

#define USED_ODOM	1
#define USED_ENC	2

/************************************************************************************************************************************/
#define PI								(3.1415926f)
#define WHEEL_CIRCUMFERENCE_MM			(184 * PI)                                    			//轮子周长
#define WHEEL_BASE						(600)													//轮距
#define REDUCTION_RATIO					(21.0f)									            	//减速比
#define ENCODE_LINE						(0x800000)										    	//编码器每圈脉冲数
#define PLUS2DIST_MM					((float)WHEEL_CIRCUMFERENCE_MM / (float)ENCODE_LINE)	//将编码器计数转化为距离mm的系数
#define POSITION_LOOP_ERROR				(1000)													//静止状态时闭环抖动允许误差
#define MAX_ENCODE_RANGE				(0x8000000000)											//多圈编码器最大范围
#define MAX_DISTANCE_PER_PERIOD			(1000 * 0.2f * REDUCTION_RATIO / PLUS2DIST_MM)     		//以最大速度(1m/s)运转计算周期时长(200ms),编码器值的最大变化量




typedef enum{
	STA_IDLE = 0,	//空闲状态,检测是否有移动指令
	STA_PLAN,		//位置规划
	STA_MOVING,		//在往目标点移动的过程中
	STA_POSTURE,	//到达目标点后姿态角度的调整
	STA_PAUSE,		//暂停移动
	STA_RESUME,		//恢复移动
	STA_STOP,		//停止移动
	STA_FAULT,		//故障状态
}Type_MotionState_Enum;

typedef enum{
	OPTION_DIRECT_CW,         //顺时针方案
	OPTION_DIRECT_CCW,        //逆时针方案
	OPTION_DIRECT_AUTO,       //自动方案
}Type_MotionOption_Enum;

typedef struct{
	float kp;
	float ki;
	float kd;
	float err_last;
	float sum;
	float out_u;
	float out_limit;
	float sum_limit;
}Type_Pid_Def;

typedef struct{
	float x;
	float y;
	float z;
}Type_Vector3_Def;

typedef struct Node {
	uint16_t x;
	uint16_t y;
	uint16_t F;
	uint16_t G;
	uint16_t H;
	
	struct Node * parent;
	struct Node * child;
}Type_Node_Def;

typedef struct {
	uint16_t count;
	void * head;
}Type_List_Def;

typedef struct{
    int length;                             //关节长度 mm
    Type_Vector3_Def pose;                  //关节位置
    Type_Vector3_Def lastPose;              //关节上次位置
    Type_Vector3_Def nextPose;              //关节下次位置(预测)
}Type_Joint_Def;

typedef struct{
	Type_MotionState_Enum motionState;      //移动状态
    Type_MotionState_Enum lastMotionState;  //上次移动状态
    Type_MotionOption_Enum motionOption;    //移动方案
    bool isFirstSwitch;                     //标识首次状态切换

    Type_Joint_Def joints[JOINT_NUM];       //关节数组

    Type_Vector3_Def robotPose;             //机器人当前位姿
    Type_Vector3_Def robotNextPose;         //机器人预测位姿
	Type_Vector3_Def robotLastPose;         //机器人上次更新时的位姿
    Type_Vector3_Def finallyTarPose;        //最终目标位姿
    Type_Vector3_Def currentTarPose;        //当前目标位姿
	
	Type_Vector3_Def homePose;        		//待机位姿

	Type_Vector3_Def robot2Pose;     		//另一个机器的位姿
	Type_Vector3_Def robot2MapPose;     	//另一个机器生成障碍物的位置
	
	Type_Vector3_Def areaMaxLimit_P;		//场地外框XY正方向最大距离
	Type_Vector3_Def areaMaxLimit_N;		//场地外框XY负方向最大距离
	
	Type_Vector3_Def areaMinLimit_P;		//场地内框XY正方向最大距离
	Type_Vector3_Def areaMinLimit_N;		//场地内框XY负方向最大距离
	
	int16_t encode3LimitMin;				//编码器3角度限制最小值
	int16_t encode3LimitMax;				//编码器3角度限制最大值
	
    int16_t minRadiusLimit;                 //最小半径限制
    int16_t maxRadiusLimit;                 //最大半径限制
	
	uint8_t **Map;							//地图内存
	uint16_t MAP_ROWS;
	uint16_t MAP_COLUMNS;
	
    Type_Pid_Def Pid_angle;                 //角度PID
    Type_Pid_Def Pid_dist;                  //距离PID
    float posAllowError;                  	//位置允许误差
    float angleAllowError;                	//角度允许误差

	bool Efence_en;						//电子围栏检测使能
    bool motorEn;                           //使能电机标志
	bool odomEn;							//测试odom时用
	bool safeEn;							//使能安全避障标志
	uint8_t efenceState;
    float vl;                               //左轮速度(前进方向为正)
    float vr;                               //右轮速度(前进方向为正)

    double millage;                         //累计里程
	int64_t	LastEncodeBuf[2];				//轮子上次编码器值

	uint32_t odomContinueTime;				//使用odom定位的持续时间(定位误差随时间增长变差)

    float motionProgress;                   //移动进度(0~100)
    float motionDistance;                   //当前命令下的总移动距离
    uint32_t poseUpdateTimeStamp;           //机器人位置更新时间戳
    uint32_t logTime;			            //log输出间隔时间
	
	uint16_t ForwardDist;					//前瞻距离
	Type_List_Def * pPathList;				//路径list

	int16_t safeDist;						//安全避障距离
}Type_Link_Def;


//系统接口
/*****************************************************************/
//joint 初始化(初始化参数,创建相关线程)
extern int xp_joint_init(void);
//joint调试接口(其依赖的编码器调试接口已包含在内)
extern int xp_joint_debug(char *type,char *fun,char *param);


//外部接口
/*****************************************************************/
//设置移动位置允许偏差(参数为负值时可获取当前设置值)
extern int xp_joint_set_pos_error(int err);
//设置旋转角度允许偏差(参数为负值时可获取当前设置值)
extern float xp_joint_set_angle_error(float err);
//设置移动时的速度(参数为负值时可获取当前设置值)
extern float xp_joint_set_motion_speed(float speed);

//获取电子围栏的触发状态
extern uint8_t xp_joint_get_efence_state(void);
//获取电子围栏的x负坐标
extern int xp_joint_get_fence_N_x(void);
//获取电子围栏的y负坐标
extern int xp_joint_get_fence_N_y(void);
//获取移动进度(0~100) 仅移动阶段,不包含角度调整阶段
extern uint8_t xp_joint_get_motion_progress(void);
//获取motion状态 (移动中,角度调整中,暂停中...)
//STA_IDLE = 0,	STA_PLAN, STA_MOVING, STA_POSTURE, STA_PAUSE, STA_RESUME, STA_STOP,	STA_FAULT = 7
extern int xp_joint_get_motion_state(void);
//获取移动状态字符串
extern const char * xp_joint_get_motion_state_str(void);
//获取机器人待机位姿
extern Type_Vector3_Def const * xp_joint_get_home_pose(void);
//设置机器人待机位姿
extern int xp_joint_set_home_pose(int x, int y, int z);
//更新另一台机器的位置
extern void xp_joint_updata_another_robot_pose(int x, int y, int z);
//获取机器人当前位姿
extern int xp_joint_get_robot_pose(int32_t * const p_x, int32_t * const p_y, float * const p_angle);
//移动是否已完成
extern bool xp_joint_is_move_complete(void);
//是否直行到某个轴坐标值
extern int xp_joint_is_straight_to_axis(float axis_x, float axis_y);


//移动机器人到场地中的某个坐标点,直接移过去(忽略任何障碍,场地规则)
extern int xp_joint_move_to_point(int32_t tar_x, int32_t tar_y, float tar_angle);
//移动机器人原地旋转到某个角度
extern int xp_joint_turn_to_angle(float tar_angle);
//移动机器人到场地中的某个坐标点,通过AStart算法自动路径规划过去
extern int xp_joint_move_to_point_by_path_AStar(int32_t tar_x, int32_t tar_y, float tar_angle);
//移动机器人到场地中的某个坐标点,通过场地固定点算法自动路径规划过去(可指定规划方向:顺时针,逆时针,自动(距离最短原则))
extern int xp_joint_move_to_point_by_path_static_point(int32_t tar_x, int32_t tar_y, float tar_angle, Type_MotionOption_Enum option);


//计算并返回获取机器人是否超出可允许运行范围,返回字节从低Bit依次是(机器人左前,左后,右后,右前点超出.管线架折叠杆角度过小)
extern uint8_t xp_joint_over_range_check(void);

//从机器人坐标系转换到场地坐标系  pSrcPoint: 待转换坐标点 pDesPoint: 转换结果  return: 转换结果地址
extern Type_Vector3_Def * const xp_joint_coordinate_transform_from_robot_to_world(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint);
//从场地坐标系转换到机器人坐标系
extern Type_Vector3_Def * const xp_joint_coordinate_transform_from_world_to_robot(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint);
//从雷达坐标系转换到机器人坐标系
extern Type_Vector3_Def * const xp_joint_coordinate_transform_from_lidar_to_robot(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint);
//从雷达坐标系转换到场地坐标系
extern Type_Vector3_Def * const xp_joint_coordinate_transform_from_lidar_to_world(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint);

//计算点到两点组成的直线的距离
extern float xp_joint_cacl_dist_point_to_line(Type_Vector3_Def const * pPoint, Type_Vector3_Def const * pLinePoint1, Type_Vector3_Def const * pLinePoint2);
//计算两点之间的角度
extern float xp_cacl_point_angle(float src_x, float src_y, float des_x, float des_y);
//计算两点之间距离
extern float caclDistance(Type_Vector3_Def const * pSrcPose, Type_Vector3_Def const * pDesPose);

//获取状态字符串
extern const char *xp_get_state_str(Type_MotionState_Enum state);
//pid计算
extern float xp_pid_cacl(Type_Pid_Def * const pid, float err);
#endif